<!DOCTYPE html>
<html>

<head>
  <meta charset="utf-8" />
  <link rel="stylesheet" href="css/animate.css">
  <link rel="stylesheet" href="css/global.css">
  <link rel="stylesheet" href="css/loading.css">
  <link rel="stylesheet" href="css/alert.css">
  <link rel="stylesheet" type="text/css" href="css/waves.min.css">
  <link rel="stylesheet" type="text/css" href="css/index.css">
  <style>


  </style>
  <script type="text/javascript" src="script/lib/jquery-3.7.1.min.js"></script>
  <script type="text/javascript" src="script/config.js"></script>
  <script type="text/javascript" src="script/file_system.js"></script>
  <script type="text/javascript" src="script/lib/three_plus.js"></script>
  <script type="text/javascript" src="script/lib/roslib.js"></script>
  <script type="text/javascript" src="script/lib/ros3d.js"></script>
  <script type="text/javascript" src="script/lib/eventemitter2.js"></script>
  <script type="text/javascript" src="script/test.js"></script>
  <script type="text/javascript" src="script/mockdata/data2.js"></script>
  <script src="js/loading.js"></script>
  <script src="js/alert.js"></script>
  <script type="text/javascript" src="script/start_slam.js"></script>
  <!-- <script type="text/javascript" src="script/demo.js"></script> -->
  <script type="text/javascript" type="text/javascript">
    var startSuccess = 0;//0-stop 1-start 
    var viewer;
    var imClient;
    var ros;
    var startEvent;
    var navGoalEvent;
    var localNavGoalEvent;
    var rosConnected = false;
    var tfClient;
    /**
     * Setup all visualization elements when the page is loaded.
     */

    function init() {
      // Connect to ROS.
      ros = new ROSLIB.Ros({
        url: 'ws://192.168.1.88:9090',
        groovyCompatibility: true //true走Goal ，false走ServiceRequest
      });

      ros.on('connection', function () {
        rosConnected = true;
        console.log('Connected to websocket server.');
        ros._sendFunc("123hhh")//发送ws测试数据

        startEvent = new ROS3D.StartEvent({ "ros": ros });
        navGoalEvent = new ROS3D.NavGoalEvent({ "ros": ros, "imClient": imClient, "viewer": viewer });
        localNavGoalEvent = new ROS3D.LocalNavGoalEvent({ "ros": ros, "imClient": imClient, "viewer": viewer });
        // startEvent.andvertise();
        navGoalEvent.subscribe();
        localNavGoalEvent.subscribe();

        $("#btn_start_fetch").css("background-color", "#01BCFF")
        // loadObj();
      });
      ros.on('error', function (error) {
        rosConnected = false;

        console.error('Error connecting to websocket server: ', error);
      });
      ros.on('close', function () {
        rosConnected = false;
        $("#btn_start_fetch").css("background-color", "#bbbbbb")
        // alert("服务器已断开")//test
        console.warn('Connection to websocket server closed.');
      });

      // Create the main viewer.
      viewer = new ROS3D.Viewer({
        divID: 'viewer',
        width: window.innerWidth,
        height: window.innerHeight,
        antialias: true,
        background: '#000000',
        cameraZoomSpeed: 5,//缩放速度
        alpha: 1, //背景色透明度
        cameraPose: {  //相机初始视角，值越大，总览范围越大
          x: -0,
          y: 0,
          z: 5
        },
        displayPanAndZoomFrame: true, //是否显示坐标轴，默认true
        // near:5,//相机远近层级的倍数，超过范围则不绘制
        // far:100,
        cameraHelper: {},
        groundMaterial: "image/test2.jpg"
      });
      globalViewer = viewer;
      // Setup a client to listen to TFs.坐标系，由服务端提供
      tfClient = new ROSLIB.TFClient({
        ros: ros,
        angularThres: 0.01,
        transThres: 0.01,
        rate: 10.0,
        fixedFrame: 'map'
      });

      tfClient.subscribe("/base_link", function (tf) {
        console.log("robotTfClient %o", tf)
        // return ;//test
        let translation = tf.translation;
        let rotation = tf.rotation;
        //todo 解析四元数应用于模型
        var q = new THREE.Quaternion(rotation.x, rotation.y, rotation.z, rotation.w)
        moveObj(translation.x * pointScaleSize, translation.y * pointScaleSize, translation.z * pointScaleSize, q);//同步放大倍数
      })

      // Setup the marker client.

      initPointCloud(0);
      mouseEvent(viewObj, imClient);//添加点击拦截事件，用于选点
      // drawDemo(imClient, viewer);//绘制本地数据集
      loadObj();//加载机器人模型
    }

    function initPointCloud(throttle_rate) {
      imClient = new ROS3D.PointCloud2({
        ros: ros,
        tfClient: tfClient,
        rootObject: viewer.scene,
        // topic: '/RGB_map_0',    // sinor本地测试数据集主题
        // topic: '/accumulated_rgb_cloud',//208验收 192.168.1.8主机 ，彩色建图
        // topic: '/cloud_registered_body',//208验收 192.168.1.6主机，黑白激光雷达
        topic: '/accumulated_rgb_cloud',
        throttle_rate: throttle_rate,
        material: { size: markerSize },
        compression: 'cbor', //压缩放肆，默认cbor
        // pointRatio:0.5, //点二次采样率，默认1  ，无差别
        max_pts: ppn,  //最大绘制点数，设置太大会有很多空点，影响渲染
        // colormap:function(e){
        //  console.log('执行自定义 colormap');
        //  return 0xffffff;
        // },

        colorsrc: 'rgb',
        finishCallback: drawDemo,
        viewer: viewer
      });
      globalPointCloud = imClient;
    }

    function drawPath() {
      //移除旧的路径
      let items = viewer.getGroupElementsByName(SELECT_OBJECTS_GROUP, "path");
      if (items && items.length == 1) {
        var arr = []
        arr.push(new THREE.Vector3(8, 8, 8))
        arr.push(new THREE.Vector3(4, 5, 6));
        console.log(items[0])
        items[0].geometry.vertices = arr;
        items[0].geometry.verticesNeedUpdate = true;
        // viewer.removeGroupElement(SELECT_OBJECTS_GROUP, items[0]);
        // imClient.drawPath(viewer, arr, 'red', 100);

      } else {
        var arr = []
        arr.push(new THREE.Vector3(1, 1, 1))
        arr.push(new THREE.Vector3(2, 2, 2))
        arr.push(new THREE.Vector3(2, 3, 3))
        arr.push(new THREE.Vector3(5, 5, 2))
        imClient.drawPath(viewer, arr, 'red', 100);
        let items = viewer.getGroupElementsByName(SELECT_OBJECTS_GROUP, "path");
        console.log(items[0])
      }
    }

    function switchMode() {
      var btn_mode_chg = document.getElementById('btn_mode_chg')
      if (globalMode == MODE_NONE) {
        globalMode = MODE_MARK;
        btn_mode_chg.innerText = "退出选点";
        $("#view_action").fadeIn();
        $("#btn_edit_mks").text("选点编辑");

      } else {
        //退出
        globalMode = MODE_NONE;
        editMarkers();
        btn_mode_chg.innerText = "选点模式";
        $("#view_action").fadeOut();
        clearAllMarker()
      }
    }

    function enterModeNew() {
      console.log("click")
      if (globalMode == MODE_NONE) {
        globalMode = MODE_MARK;
        $("#top_cover").fadeIn();
        $("#right_menu").fadeIn();
        // $("#right_cover").fadeIn();
        // $("#left_cover").fadeOut();
        $("#left_menu").fadeOut();

      }
    }

    function exitModeNew() {
      //退出
      globalMode = MODE_NONE;
      $("#top_cover").fadeOut();
      // $("#right_cover").fadeOut();
      $("#right_menu").fadeOut();

      // $("#left_cover").fadeIn();
      $("#left_menu").fadeIn();
      clearAllMarker()
    }

    function clearAllMarker() {
      imClient.clearAllMarker(viewer);
      if (globalMode != MODE_NONE) {
        globalMode = MODE_MARK;
      }
      editMarkers();
    }

    function editMarkers() {
      // if (globalMode==MODE_NONE) {
      //   return
      // }
      // if(globalMode == MODE_MARK){
      //   globalMode = MODE_EDIT_MARK;
      //   $("#btn_edit_mks").text( "退出编辑" );
      //   imClient.editMarkers(viewer, true) //切换编辑模式
      // }else if(globalMode == MODE_EDIT_MARK){
      //   globalMode = MODE_MARK;
      //   $("#btn_edit_mks").text("选点编辑");
      //   imClient.editMarkers(viewer, false) //切换编辑模式
      // }
    }

    var timer;
    var tempInt = 0;
    function resetCamera() {
      viewer.reset();


      // if(timer){
      //   clearInterval(timer)
      //   timer = undefined;
      // }else{
      //   timer = setInterval(()=>{
      //     tempInt++;
      //     console.log(tempInt)
      //     drawDemo(imClient,viewer,tempInt)
      //   },1000)
      // }


      //  tempInt=tempInt+10;
      //   console.log(tempInt)
        // drawDemo(imClient,viewer,tempInt)


        //测试导航路径代码
        // if(!navGoalEvent){
        //   navGoalEvent = new ROS3D.NavGoalEvent({ "ros": ros, "imClient": imClient, "viewer": viewer });
        //   localNavGoalEvent = new ROS3D.LocalNavGoalEvent({ "ros": ros, "imClient": imClient, "viewer": viewer });
        // }
        // navGoalEvent.testPath();
        // localNavGoalEvent.testPath();
      
    }

    function loadObj() {
      // viewer.loadObj();
      viewer.loadSTLObject();//test
    }


    const modifyQ = new THREE.Quaternion();
    modifyQ.setFromAxisAngle(new THREE.Vector3(0, 0, 1), Math.PI / 2);//Z轴逆时针旋转90°
    function moveObj(x, y, z, q) {
      viewer.moveObj(x,y,z);
      viewer.rotateObj(q, modifyQ);//再乘以modifyQ 修正旋转
    }

    function sendService() {
      console.log("globalAxeObj quaternion:%o", globalAxeObj.quaternion);
      if (!rosConnected) {
        // alert("服务器未连接");
        // return;
      }
      if (ros) {
        let name = "nav_goal";
        // let name = "/move_base_simple/goal";
        let type = "rospy_tutorials/AddTwoIntsRequest222";
        let req = {}
        let data = {}
        // if (!viewer.groups[SELECT_OBJECTS_GROUP].children) {
        //   console.log("未选中任何点...")
        //   return;
        // }
        // let children = viewer.groups[SELECT_OBJECTS_GROUP].children;
        // if (children.length == 0) {
        //   alert("目标点为空")
        //   return;
        // }
        // data.points = points;
        // req.x = children[0].position.x/pointScaleSize;
        // req.y = children[0].position.y/pointScaleSize;
        // req.z = children[0].position.z/pointScaleSize;

        req.x = imClient.targetVec3.x;
        req.y = imClient.targetVec3.y;
        req.z = imClient.targetVec3.z;

        req.q0 = globalAxeObj.quaternion.x;
        req.q1 = globalAxeObj.quaternion.y;
        req.q2 = globalAxeObj.quaternion.z;
        req.q3 = globalAxeObj.quaternion.w;
        data.req = JSON.stringify(req);
        console.log(data);
        ros.sendCustomService(name, type, data, function (result) {
          globalMode == MODE_NONE;
          console.log("btn_send_service success,result is %o", result);//成功的回调
        },
          function (error) {
            console.error("fail msg %o", error);//失败的回调
          });

        // navGoalEvent.publish(points[0]);
      }
    }
    function startFetch() {
      if (!rosConnected) {
        alert("服务器未连接");
        return;
      }
      $('body').loading({
        loadingWidth: 240,
        title: '请稍等!',
        name: 'start',
        discription: startSuccess == 0 ? '正在启动建图...' : "正在停止...",
        direction: 'column',
        type: 'origin',
        // originBg:'#71EA71',
        originDivWidth: 40,
        originDivHeight: 40,
        originWidth: 6,
        originHeight: 6,
        smallLoading: false,
        loadingMaskBg: 'rgba(0,0,0,0.2)',

      });
      let name = "start_slam";
      let type = "slamnav_ctrl/SlamNavCtrl";
      let points = [];
      let data;

      if (startSuccess == 0) {
        data = { "req": "start" }
        // ros.sendCustomService(name,type,data,function(result){
        //     removeLoading('start');
        //     console.log("sendCustomService start_slam,result is %o",result);//成功的回调
        //     if("success"==result.res){
        //       startSuccess = 1;
        //       $("#btn_start_fetch").text("停止建图") ;
        //       $("#btn_start_fetch").css("background-color","#F44E5A")
        //     }else{
        //       alert("开始建图失败，错误信息:"+result.res)
        //     }
        //   },
        //   function(error){
        //     removeLoading('start');
        //     console.error("start_slam fail msg %o",error);//失败的回调
        //     alert(error)
        //   });
        startSlam(ros, name, type, data, function (result) {
          removeLoading('start');
          console.log("sendCustomService start_slam,result is %o", result);//成功的回调
          if ("success" == result.res) {
            startSuccess = 1;
            $("#btn_start_fetch").text("停止建图");
            $("#btn_start_fetch").css("background-color", "#F44E5A")
          } else {
            //后台轮询，不做处理
          }
        },
          function (error) {
            removeLoading('start');
            console.error("start_slam fail msg %o", error);//失败的回调
            alert(error)
          })
      } else {
        data = { "req": "stop" }
        startSlam(ros, name, type, data, function (result) {
          removeLoading('start');
          console.log("btn_send_service success,result is %o", result);//成功的回调
          if ("success" == result.res) {
            startSuccess = 0;
            $("#btn_start_fetch").text("开始建图");
            $("#btn_start_fetch").css("background-color", "#01BCFF")
          } else {
            //后台轮询，不做处理
          }
        },
          function (error) {
            removeLoading('start');
            console.error("start_slam fail msg %o", error);//失败的回调
            alert(error)
          });
        // ros.sendCustomService(name,type,data,function(result){
        //     removeLoading('start');
        //     console.log("btn_send_service success,result is %o",result);//成功的回调
        //     if("success"==result.res){
        //       startSuccess = 0;
        //       $("#btn_start_fetch").text("开始建图") ;
        //       $("#btn_start_fetch").css("background-color","#01BCFF")
        //     }else{
        //       alert("停止建图失败，错误信息:"+result.res)
        //     }1
        //   },
        //   function(error){
        //     removeLoading('start');
        //     console.error("start_slam fail msg %o",error);//失败的回调
        //     alert(error)
        //   });
      }

    }



    function reconnect() {
      if (ros) {
        ros.close();
        ros.connect(ros.url);
      }
    }


    function throttleRate() {
      var html = '<input type="number" name="pwd" placeholder="该值用于控制网络传输流量，节流值越大，传输流量越小，默认值为零）" style="text-align:center" />'
      $.confirm({ title: '网络流量控制', content: html }, function () {
        var password = $('input[name="pwd"]').val();
        if (!password) {
          password = "0";
        }

        $('#btn_throttle_rate').text("节流设置(" + password + ")")
        var rate = parseInt(password);
        imClient.throttle_rate = rate;
        imClient.subscribe();//重新订阅，使节流速率生效
        $.toast("设置成功");
      });
    }

  



  </script>


</head>

<body onload="init()">

  <div id="viewer"></div>
  <div id="top_cover" style="display: none;">
    <div style="width: 50px;height: 50px;border: #bbbbbb 1px;left: 50px;position: absolute; ">
      <img src="image/exit.svg" style="width: 100%;height:100% ;" onclick="exitModeNew()" />
    </div>
    <div style="color: white;font-size: 24px;">选点模式</div>
  </div>
  <div id="left_cover"></div>
  <div id="right_cover"></div>

  <div id="left_menu" class="float_layout ic_menu " style="background-color: transparent;">
    <div class="ic_item" style="background-color: rgba(236, 229, 229, 0.5);border-radius: 50%;">
      <img src="image/start.svg" style="width: 100%;height:100% ;" onclick="startFetch()" />
      <div class="txt_item">开始建图</div>
    </div>
    <div class="ic_item">
      <img src="image/view_reset.svg" style="width: 100%;height:100% ;" onclick="resetCamera()" />
      <div class="txt_item">重置视角</div>
    </div>
    <div class="ic_item">
      <img src="image/rate.svg" style="width: 100%;height:100% ;" onclick="throttleRate()" />
      <div class="txt_item">流量控制</div>
    </div>
    <div class="ic_item" onclick="enterModeNew()">
      <img src="image/mark.svg" style="width: 100%;height:100% ;" />
      <div class="txt_item">选点模式</div>
    </div>
<!--
    <div class="ic_item" onclick="loadFile()">
      <img src="image/mark.svg" style="width: 100%;height:100% ;" />
      <input type="file" class="txt_item" webkitdictory multiple>加载文件</div>
      <div class="txt_item">加载文件</div>
    </div>
  -->
  </div>

  <div id="right_menu" class="float_layout ic_menu "
    style="display: none; background-color: transparent;right: 0;margin-right: 50px;margin-left: 0px;margin-top: 150px;">
    <div class="ic_item">
      <img src="image/clear.svg" style="width: 100%;height:100% ;" onclick="clearAllMarker()" />
      <div class="txt_item">清除选点</div>
    </div>

    <div class="ic_item">
      <img src="image/send.svg" style="width: 100%;height:100% ;" onclick="sendService()" />
      <div class="txt_item">发送目标</div>
    </div>
  </div>

  <!-- 改为图标形式 -->
  <div class="float_layout" style="background-color:transparent;display: none;">
    <button class="btn float-button-light waves-effect waves-button waves-float waves-light" id="btn_start_fetch"
      onclick="startFetch()" style="background-color: #bbbbbb;display: block;">开始建图</button>
    <button class="btn float-button-light waves-effect waves-button waves-float waves-light" id="btn_reset"
      onclick="resetCamera()" style="background-color: #01BCFF;display: block;">视角复位</button>
    <button class="btn float-button-light waves-effect waves-button waves-float waves-light" id="btn_reset"
      onclick="reconnect()" style="background-color: #01BCFF;display: block;">刷新连接</button>
    <button class="btn float-button-light waves-effect waves-button waves-float waves-light" id="btn_throttle_rate"
      onclick="throttleRate()" style="background-color: #01BCFF;display: block;">节流设置</button>
    <button class="btn float-button-light waves-effect waves-button waves-float waves-light" id="btn_mode_chg"
      onclick="switchMode()" style="background-color: #01BCFF;">选点模式</button>
    <input class="btn float-button-light waves-effect waves-button waves-float waves-light" id="file_path"
      onclick="loadFile()" style="background-color: #01BCFF;" type="file" webkitdictory multiple>加载地图</button>
    <button class="btn float-button-light waves-effect waves-button waves-float waves-light" id="btn_throttle_rate"
      onclick="throttleRate()" type="file" style="background-color: #01BCFF;display: block;">节流设置</button>
    <div id="view_action" style="display: none;">
      <!-- <button class="btn" id="btn_path" onclick="drawPath()"
        style="background-color: white;display: block;">绘制线条</button> -->
      <button class="btn float-button-light waves-effect waves-button waves-float waves-light" id="btn_clear_mks"
        onclick="clearAllMarker()" style="background-color: #01BCFF;display: block">清空选点</button>
      <!-- <button class="btn" id="btn_edit_mks" onclick="editMarkers()"
        style="background-color: white;display: block">编辑选点</button> -->

      <button class="btn float-button-light waves-effect waves-button waves-float waves-light" id="btn_send_service"
        onclick="sendService()" style="background-color: #01BCFF;display: block">发送导航点</button>
    </div>
  </div>

  <script>
    //  var btn = document.getElementById('btn');
    // btn.onclick = function(e){
    //   console.log(viewer.camera.worldMatrix);
    // }

  </script>
</body>

</html>